#include "at32f4xx.h"
#include "motor.h"
#include "GPIO_Configuration.h"
#include "maths.h"
#include "USART_Configuration.h"

#define TMR_CLOCK_DIV (150 - 1)
#define TMR_CLOCK_PERIOD 20000
#define TMRx_CHANNLEx_INIT(T, C) TMR_OC##C##Init(T, &TMRx_OCInitStructure);

static TMR_TimerBaseInitType TMRx_baseInitStructure;
static TMR_OCInitType TMRx_OCInitStructure;

motor_port_property_s motor_port[MOTOR_PORT_COUNT];
void TMRx_Clock_Init(TMR_Type* TMRx, uint32_t _DIV, uint32_t _Period)
{
	TMR_Reset(TMRx);
	TMR_TimeBaseStructInit(&TMRx_baseInitStructure);
	TMRx_baseInitStructure.TMR_DIV = _DIV;
	TMRx_baseInitStructure.TMR_ClockDivision = TMR_CKD_DIV1;
	TMRx_baseInitStructure.TMR_Period = _Period;
	TMRx_baseInitStructure.TMR_CounterMode = TMR_CounterDIR_Up;
	// TMR1_baseInitStructure.TMR_RepetitionCounter = 4;
	TMR_TimeBaseInit(TMRx, &TMRx_baseInitStructure);
}
// void TMRx_PWM_Channelx_Init(TMR_Type *TMRx)
// {

// 	TMR_OCStructInit(&TMRx_OCInitStructure);
// 	TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
// 	TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
// 	TMRx_OCInitStructure.TMR_Pulse = 0;
// 	TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;
// 	if (TMRx == TMR1)
// 		TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Disable;
// }
/**
 * @brief  设置主电机的属性
 * @note
 * @param  driveType: BRUSH_MOTOR, BRUSHLESS_MOTOR
 * @param  freq: PWM frequency(max 150KHZ)
 * @param  minDuty: 0~1000
 * @param  maxDuty: 0~1000
 * @retval None
 */
void main_motor_init(DRIVE_TYPE_E driveType, uint16_t freq, uint16_t minDuty, uint16_t maxDuty)
{
	freq = constrain(freq, 3, 150000);
	switch (driveType)
	{
	case BRUSH_MOTOR:
		// Serial_printf(&serialHandle[serial1], (const char*)"BRUSH_MOTOR\n");
		motor_port[MAIN_MOTOR].clock_period = 1000;
		motor_port[MAIN_MOTOR].clock_div = (SystemCoreClock / motor_port[MAIN_MOTOR].clock_period / freq - 1);
		/* code */
		break;

	case BRUSHLESS_MOTOR:
		/* code */
		break;

	default:
		break;
	}
	TMR_OCStructInit(&TMRx_OCInitStructure);
	TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
	TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
	TMRx_OCInitStructure.TMR_Pulse = 0;
	TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;

	motor_port[MAIN_MOTOR].minDuty = minDuty;
	motor_port[MAIN_MOTOR].maxDuty = maxDuty;
	TMRx_Clock_Init(TMR3, motor_port[MAIN_MOTOR].clock_div, motor_port[MAIN_MOTOR].clock_period);
	TMRx_Clock_Init(TMR4, motor_port[MAIN_MOTOR].clock_div, motor_port[MAIN_MOTOR].clock_period);
	// Serial_printf(&serialHandle[serial1], (const char*)"minDuty:%d\n",minDuty);
	// Serial_printf(&serialHandle[serial1], (const char*)"minDuty:%d\n",maxDuty);
	// Serial_printf(&serialHandle[serial1], (const char*)"minDuty:%d\n",motor_port[MAIN_MOTOR].minDuty);
	// Serial_printf(&serialHandle[serial1], (const char*)"maxDuty:%d\n",motor_port[MAIN_MOTOR].maxDuty);
	TMRx_CHANNLEx_INIT(TMR3, 1);
	TMRx_CHANNLEx_INIT(TMR3, 2);
	TMRx_CHANNLEx_INIT(TMR3, 3);
	TMRx_CHANNLEx_INIT(TMR3, 4);

	TMRx_CHANNLEx_INIT(TMR4, 1);
	TMRx_CHANNLEx_INIT(TMR4, 2);
	TMRx_CHANNLEx_INIT(TMR4, 3);
	TMRx_CHANNLEx_INIT(TMR4, 4);

	TMR_CtrlPWMOutputs(TMR3, ENABLE);
	TMR_CtrlPWMOutputs(TMR4, ENABLE);

	TMR_Cmd(TMR3, ENABLE);
	TMR_Cmd(TMR4, ENABLE);
}
/**
 * @brief 扩展电机口初始化
 *
 * @param driveType 分为有刷电机（BRUSH_MOTOR）与无刷电机（BRUSHLESS_MOTOR）
 * @param freq BRUSH_MOTOR：3~150000Hz，BRUSHLESS_MOTOR：10~50000Hz
 * @param minDuty BRUSH_MOTOR：0~1000,BRUSHLESS_MOTOR：0~1000000/FREQ < maxDuty
 * @param maxDuty BRUSH_MOTOR：0~1000,BRUSHLESS_MOTOR：0~1000000/FREQ > minDuty
 */
void extend_motor_init(DRIVE_TYPE_E driveType, uint16_t freq, uint16_t minDuty, uint16_t maxDuty)
{

	switch (driveType)
	{
	case BRUSH_MOTOR:
		freq = constrain(freq, 3, 150000); //目前只支持3~150000Hz
		// Serial_printf(&serialHandle[serial1], (const char*)"BRUSH_MOTOR\n");
		motor_port[EXTEND_MOTOR].clock_period = 1000;
		motor_port[EXTEND_MOTOR].clock_div = (SystemCoreClock / motor_port[EXTEND_MOTOR].clock_period / freq - 1);
		/* code */
		break;

	case BRUSHLESS_MOTOR:
		freq = constrain(freq, 16, 50000); //目前只支持10~50000Hz
		/*时钟频率应该是1MHz,保证1us的PWM精度*/
		motor_port[EXTEND_MOTOR].clock_period = 1000000L / freq;
		motor_port[EXTEND_MOTOR].clock_div = (SystemCoreClock / motor_port[EXTEND_MOTOR].clock_period / freq - 1);
		/* code */
		break;

	default:
		break;
	}
	motor_port[EXTEND_MOTOR].minDuty = minDuty;
	motor_port[EXTEND_MOTOR].maxDuty = maxDuty;

	if (CHECK_EXTAND_PORT(EX_USE_OD1) || CHECK_EXTAND_PORT(EX_USE_OD2) || CHECK_EXTAND_PORT(EX_USE_OD3) || CHECK_EXTAND_PORT(EX_USE_OD4))
	{
#ifdef ZINO_I
#define EX_MOTOR_TMR TMR1
		RCC->APB2EN |= RCC_APB2PERIPH_TMR1; // OD1~4
#elif defined(ZINO_PREMIUM)
#define EX_MOTOR_TMR TMR8
		RCC->APB2EN |= RCC_APB2PERIPH_TMR8; // OD1~4
#elif defined(ZINO_NP)
#define EX_MOTOR_TMR TMR8
		RCC->APB2EN |= RCC_APB2PERIPH_TMR8; // OD1~4
#else
#error "Please define ZINO_DEVICE for EX MOTOR PORT!"
#endif

		// rt_kprintf("EX OD LIST:\n");
		// rt_kprintf("EX_USE_OD1:%d\n", CHECK_EXTAND_PORT(EX_USE_OD1));
		// rt_kprintf("EX_USE_OD2:%d\n", CHECK_EXTAND_PORT(EX_USE_OD2));
		// rt_kprintf("EX_USE_OD3:%d\n", CHECK_EXTAND_PORT(EX_USE_OD3));
		// rt_kprintf("EX_USE_OD4:%d\n", CHECK_EXTAND_PORT(EX_USE_OD4));
		// Serial_flush(&serialHandle[serial1]);

		TMRx_Clock_Init(EX_MOTOR_TMR, motor_port[EXTEND_MOTOR].clock_div, motor_port[EXTEND_MOTOR].clock_period);
		TMR_OCStructInit(&TMRx_OCInitStructure);
		TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
		TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
		TMRx_OCInitStructure.TMR_Pulse = 0;
		TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;

		if (CHECK_EXTAND_PORT(EX_USE_OD1))
			TMRx_CHANNLEx_INIT(EX_MOTOR_TMR, 1);
		if (CHECK_EXTAND_PORT(EX_USE_OD2))
			TMRx_CHANNLEx_INIT(EX_MOTOR_TMR, 2);
		if (CHECK_EXTAND_PORT(EX_USE_OD3))
			TMRx_CHANNLEx_INIT(EX_MOTOR_TMR, 3);
		if (CHECK_EXTAND_PORT(EX_USE_OD4))
			TMRx_CHANNLEx_INIT(EX_MOTOR_TMR, 4);

		TMR_CtrlPWMOutputs(EX_MOTOR_TMR, ENABLE);
		TMR_Cmd(EX_MOTOR_TMR, ENABLE);
	}
}
/**
 * @brief 扩展口PWM初始化
 *
 * @param driveType 分为有刷电机（BRUSH_MOTOR）与无刷电机（BRUSHLESS_MOTOR）
 * @param freq BRUSH_MOTOR：3~150000Hz，BRUSHLESS_MOTOR：10~50000Hz
 * @param minDuty BRUSH_MOTOR：0~1000,BRUSHLESS_MOTOR：0~1000000/FREQ < maxDuty
 * @param maxDuty BRUSH_MOTOR：0~1000,BRUSHLESS_MOTOR：0~1000000/FREQ > minDuty
 */
void extend_pwm_init(DRIVE_TYPE_E driveType, uint16_t freq, uint16_t minDuty, uint16_t maxDuty)
{

	switch (driveType)
	{
	case BRUSH_MOTOR:
		freq = constrain(freq, 3, 150000); //目前只支持3~150000Hz
		// Serial_printf(&serialHandle[serial1], (const char*)"BRUSH_MOTOR\n");
		motor_port[EXTEND_PWM].clock_period = 1000;
		motor_port[EXTEND_PWM].clock_div = (SystemCoreClock / motor_port[EXTEND_PWM].clock_period / freq - 1);
		/* code */
		break;

	case BRUSHLESS_MOTOR:
		freq = constrain(freq, 16, 50000); //目前只支持10~50000Hz
		/*时钟频率应该是1MHz,保证1us的PWM精度*/
		motor_port[EXTEND_PWM].clock_period = 1000000L / freq;
		motor_port[EXTEND_PWM].clock_div = (SystemCoreClock / motor_port[EXTEND_PWM].clock_period / freq - 1);
		/* code */
		break;

	default:
		break;
	}
	motor_port[EXTEND_PWM].minDuty = minDuty;
	motor_port[EXTEND_PWM].maxDuty = maxDuty;

	if (CHECK_EXTAND_PORT(EX_USE_PWM5) || CHECK_EXTAND_PORT(EX_USE_PWM6))
	{
		RCC->APB1EN |= RCC_APB1PERIPH_TMR2; // PWM5~6
		RCC_APB1PeriphResetCmd(RCC_APB1PERIPH_TMR2, ENABLE);
		RCC_APB1PeriphClockCmd(RCC_APB1PERIPH_TMR2, ENABLE);

		// rt_kprintf("EX_USE_PWM5:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM5));
		// rt_kprintf("EX_USE_PWM6:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM6));
		// Serial_flush(&serialHandle[serial1]);
		// rt_kprintf("EX_USE_OD3:%d\n",CHECK_EXTAND_PORT(EX_USE_OD3));
		// rt_kprintf("EX_USE_OD4:%d\n",CHECK_EXTAND_PORT(EX_USE_OD4));
		TMRx_Clock_Init(TMR2, motor_port[EXTEND_PWM].clock_div, motor_port[EXTEND_PWM].clock_period);
		TMR_OCStructInit(&TMRx_OCInitStructure);
		TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
		TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
		TMRx_OCInitStructure.TMR_Pulse = 0;
		TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;

		if (CHECK_EXTAND_PORT(EX_USE_PWM5))
			TMRx_CHANNLEx_INIT(TMR2, 3);
		if (CHECK_EXTAND_PORT(EX_USE_PWM6))
			TMRx_CHANNLEx_INIT(TMR2, 4);
		TMR_CtrlPWMOutputs(TMR2, ENABLE);
		TMR_Cmd(TMR2, ENABLE);
	}
	if (CHECK_EXTAND_PORT(EX_USE_PWM1) || CHECK_EXTAND_PORT(EX_USE_PWM2) || CHECK_EXTAND_PORT(EX_USE_PWM3) || CHECK_EXTAND_PORT(EX_USE_PWM4))
	{
		RCC_APB1PeriphResetCmd(RCC_APB1PERIPH_TMR5, ENABLE);
		RCC_APB1PeriphClockCmd(RCC_APB1PERIPH_TMR5, ENABLE); // PWM1~4

		// rt_kprintf("EX_USE_PWM1:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM1));
		// rt_kprintf("EX_USE_PWM2:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM2));
		// rt_kprintf("EX_USE_PWM3:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM3));
		// rt_kprintf("EX_USE_PWM4:%d\n", CHECK_EXTAND_PORT(EX_USE_PWM4));
		// Serial_flush(&serialHandle[serial1]);

		TMRx_Clock_Init(TMR5, motor_port[EXTEND_PWM].clock_div, motor_port[EXTEND_PWM].clock_period);

		TMR_OCStructInit(&TMRx_OCInitStructure);
		TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
		TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
		TMRx_OCInitStructure.TMR_Pulse = 0;
		TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;

		if (CHECK_EXTAND_PORT(EX_USE_PWM1))
			TMRx_CHANNLEx_INIT(TMR5, 1);
		if (CHECK_EXTAND_PORT(EX_USE_PWM2))
			TMRx_CHANNLEx_INIT(TMR5, 2);
		if (CHECK_EXTAND_PORT(EX_USE_PWM3))
			TMRx_CHANNLEx_INIT(TMR5, 3);
		if (CHECK_EXTAND_PORT(EX_USE_PWM3))
			TMRx_CHANNLEx_INIT(TMR5, 4);
		TMR_CtrlPWMOutputs(TMR5, ENABLE);
		TMR_Cmd(TMR5, ENABLE);
	}
}

/**
 * @brief  设置主机各个的油门
 * @note   已匹配现在主要用的无人机电机，正油门时正转。2021-1-8 15:21:28 by lvgrape
 * @param  port: MAIN_MOTOR_1，MAIN_MOTOR_2，MAIN_MOTOR_3，MAIN_MOTOR_4
 * @param  throttle: -1000~1000
 * @retval None
 */
void main_motor_port_output(MAIN_MOTORS_e port, int16_t throttle)
{
	// throttle = constrain(throttle,-1000,1000);
	// Serial_printf(&serialHandle[serial1], (const char*)"throttle:%d\n",throttle);
	throttle = throttle * (motor_port[MAIN_MOTOR].maxDuty - motor_port[MAIN_MOTOR].minDuty) / 1000 + motor_port[MAIN_MOTOR].minDuty;
	throttle = constrain(throttle, -1000, 1000);
	// Serial_printf(&serialHandle[serial1], (const char*)"throttle:%d\n",throttle);

	switch (port)
	{
	case MAIN_MOTOR_1:
		TMR3->CC1 = throttle > 0 ? throttle : 0;
		TMR4->CC1 = throttle < 0 ? -throttle : 0;
		break;
	case MAIN_MOTOR_2:
		TMR3->CC2 = throttle > 0 ? throttle : 0;
		TMR4->CC2 = throttle < 0 ? -throttle : 0;
		break;
	case MAIN_MOTOR_3:
		TMR3->CC3 = throttle > 0 ? throttle : 0;
		TMR4->CC3 = throttle < 0 ? -throttle : 0;
		break;
	case MAIN_MOTOR_4:
		TMR3->CC4 = throttle > 0 ? throttle : 0;
		TMR4->CC4 = throttle < 0 ? -throttle : 0;
		break;
	default:
		break;
	}
}
void extend_motor_port_output(EXTEND_MOTORS_E port, int16_t throttle)
{
	// throttle = constrain(throttle,-1000,1000);
	// Serial_printf(&serialHandle[serial1], (const char *)"throttle:%d\n", throttle);
	throttle = throttle * (motor_port[EXTEND_MOTOR].maxDuty - motor_port[EXTEND_MOTOR].minDuty) / 1000 + motor_port[EXTEND_MOTOR].minDuty;
	throttle = constrain(throttle, 0, 1000);
	// Serial_printf(&serialHandle[serial1], (const char *)"throttle:%d\n", throttle);
#ifdef ZINO_I
#define EX_MOTOR_TMR TMR1
#elif defined(ZINO_PREMIUM) || defined(ZINO_NP)
#define EX_MOTOR_TMR TMR8
#else
#error "Please define ZINO_DEVICE for EX MOTOR PORT!"
#endif
	switch (port)
	{
	case EXTEND_MOTOR_1:
		EX_MOTOR_TMR->CC1 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_MOTOR_2:
		EX_MOTOR_TMR->CC2 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_MOTOR_3:
		EX_MOTOR_TMR->CC3 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_MOTOR_4:
		EX_MOTOR_TMR->CC4 = throttle > 0 ? throttle : 0;
		break;
	default:
		break;
	}
}
void extend_pwm_port_output(EXTEND_PWM_e port, int16_t throttle)
{
	throttle = constrain(throttle, -1000, 1000);
	// Serial_printf(&serialHandle[serial1], (const char*)"throttle:%d\n",throttle);
	// throttle = throttle * (motor_port[EXTEND_PWM].maxDuty - motor_port[EXTEND_PWM].minDuty) / 1000 + motor_port[EXTEND_PWM].minDuty;
	// throttle = constrain(throttle, motor_port[EXTEND_PWM].minDuty, motor_port[EXTEND_PWM].maxDuty);
	// Serial_printf(&serialHandle[serial1], (const char*)"throttle:%d\n",throttle);
	// rt_kprintf("EXTEND_PWM_6:%d\t",throttle);
	if (motor_port[EXTEND_PWM].state & (0x01 << port))
	{
		throttle = throttle * (motor_port[EXTEND_PWM].maxDuty - motor_port[EXTEND_PWM].minDuty) / 1000 + motor_port[EXTEND_PWM].minDuty;
	}
	else
	{
		throttle = 0;
	}

	switch (port)
	{
	case EXTEND_PWM_1:
		TMR5->CC1 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_PWM_2:
		TMR5->CC2 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_PWM_3:
		TMR5->CC3 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_PWM_4:
		TMR5->CC4 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_PWM_5:
		TMR2->CC3 = throttle > 0 ? throttle : 0;
		break;
	case EXTEND_PWM_6:
		TMR2->CC4 = throttle > 0 ? throttle : 0;
		break;
	default:
		break;
	}

}
void extend_pwm_port_state(EXTEND_PWM_e port, bool on_off)
{
	if (on_off)
	{
		motor_port[EXTEND_PWM].state |= ((uint8_t)0x01 << port);
	}
	else
	{
		motor_port[EXTEND_PWM].state &= ~((uint8_t)0x01 << port);
	}

	// switch (port)
	// {
	// case EXTEND_PWM_1:

	// 	TMR5->CC1 = on_off;

	// 	break;
	// case EXTEND_PWM_2:
	// 	TMR5->CC2 = on_off;

	// 	break;
	// case EXTEND_PWM_3:
	// 	TMR5->CC3 = on_off;

	// 	break;
	// case EXTEND_PWM_4:
	// 	TMR5->CC4 = on_off;

	// case EXTEND_PWM_5:
	// 	TMR2->CC3 = on_off;

	// case EXTEND_PWM_6:
	// 	TMR2->CC4 = on_off;
	// 	break;
	// default:
	// 	break;
	// }
}
bool get_extend_pwm_port_state(EXTEND_PWM_e port)
{
	return motor_port[port].state & ((uint8_t)(0x01 << port));
}
void extend_motor_test(uint16_t m1, uint16_t m2, uint16_t m3, uint16_t m4)
{
	extend_motor_port_output(EXTEND_MOTOR_1, m1);
	extend_motor_port_output(EXTEND_MOTOR_2, m2);
	extend_motor_port_output(EXTEND_MOTOR_3, m3);
	extend_motor_port_output(EXTEND_MOTOR_4, m4);
}
void motor_output_init_all(MOTOR_PORTS_e motorPort, DRIVE_TYPE_E diriveType, uint16_t freq, uint16_t minDuty, uint16_t maxDuty)
{
	// Serial_printf(&serialHandle[serial1], (const char*)"motor_output_init_all minDuty:%d\n",minDuty);
	// Serial_printf(&serialHandle[serial1], (const char*)"maxDuty:%d\n",maxDuty);
	switch (motorPort)
	{
	case MAIN_MOTOR:
		main_motor_init(diriveType, freq, minDuty, maxDuty);
		break;
	case EXTEND_MOTOR:
		extend_motor_init(diriveType, freq, minDuty, maxDuty);
		break;
	case EXTEND_PWM:
		extend_pwm_init(diriveType, freq, minDuty, maxDuty);
		break;
		/* code */

	default:
		break;
	}
}
// void motor_init(void)
// {

// 	// TMRx_Clock_Init(TMR1, TMR_CLOCK_DIV, TMR_CLOCK_PERIOD);//150MHz/150/20000=50Hz
// 	// TMRx_Clock_Init(TMR2, TMR_CLOCK_DIV, TMR_CLOCK_PERIOD);//150MHz/150/20000=50Hz
// 	TMRx_Clock_Init(TMR3, 14, TMR_CLOCK_PERIOD); //150MHz/15/20000=500Hz
// 	TMRx_Clock_Init(TMR4, 14, TMR_CLOCK_PERIOD); //150MHz/15/20000=500Hz

// 	TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_PWM1;
// 	TMRx_OCInitStructure.TMR_OCPolarity = TMR_OCPolarity_High;
// 	TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Enable;
// 	TMRx_OCInitStructure.TMR_Pulse = 0;
// 	TMRx_OCInitStructure.TMR_OCIdleState = TMR_OCIdleState_Reset;
// 	TMRx_OCInitStructure.TMR_OCNIdleState = TMR_OCIdleState_Reset;
// 	TMRx_OCInitStructure.TMR_OutputNState = TMR_OutputNState_Disable;
// 	TMRx_OCInitStructure.TMR_OCNPolarity = TMR_OCNPolarity_High;

// 	TMRx_CHANNLEx_INIT(TMR2, 1);
// 	TMRx_CHANNLEx_INIT(TMR2, 2);
// 	TMRx_CHANNLEx_INIT(TMR3, 1);
// 	TMRx_CHANNLEx_INIT(TMR3, 2);
// 	TMRx_CHANNLEx_INIT(TMR3, 3);
// 	TMRx_CHANNLEx_INIT(TMR3, 4);
// 	TMRx_CHANNLEx_INIT(TMR4, 1);
// 	TMRx_CHANNLEx_INIT(TMR4, 2);
// 	TMRx_CHANNLEx_INIT(TMR4, 3);
// 	TMRx_CHANNLEx_INIT(TMR4, 4);

// 	TMRx_OCInitStructure.TMR_OCMode = TMR_OCMode_Inactive;
// 	TMRx_OCInitStructure.TMR_OutputState = TMR_OutputState_Disable;
// 	TMRx_CHANNLEx_INIT(TMR1, 1);
// 	TMRx_CHANNLEx_INIT(TMR1, 2);
// 	TMRx_CHANNLEx_INIT(TMR1, 3);

// 	// TMR_CtrlPWMOutputs(TMR1, ENABLE);
// 	TMR_CtrlPWMOutputs(TMR2, ENABLE);
// 	TMR_CtrlPWMOutputs(TMR3, ENABLE);
// 	TMR_CtrlPWMOutputs(TMR4, ENABLE);

// 	//TMR1 模拟PWM
// 	TMR_INTConfig(TMR1, TMR_INT_CC1 | TMR_INT_CC2 | TMR_INT_CC3, ENABLE); /*用中断控制模拟PWM输出*/
// 	TMR_INTConfig(TMR1, TMR_INT_Overflow, ENABLE);

// 	TMR_Cmd(TMR1, ENABLE);
// 	TMR_Cmd(TMR2, ENABLE);
// 	TMR_Cmd(TMR3, ENABLE);
// 	TMR_Cmd(TMR4, ENABLE);
// }

// void motor_speed(MOTOR_TYPE motorx, int16_t speed)
// {
// 	if (speed > 1000)
// 		speed = 1000;
// 	if (speed < -1000)
// 		speed = -1000;
// 	int16_t value = speed * TMR_CLOCK_PERIOD / 1000;
// 	switch (motorx)
// 	{
// 	case MOTOR_1:
// 		if (value >= 0)
// 		{
// 			TMR3->CC1 = value;
// 			TMR4->CC1 = 0;
// 		}
// 		else
// 		{
// 			TMR4->CC1 = -value;
// 			TMR3->CC1 = 0;
// 		}
// 		break;
// 	case MOTOR_2:
// 		if (value >= 0)
// 		{
// 			TMR3->CC2 = value;
// 			TMR4->CC2 = 0;
// 		}
// 		else
// 		{
// 			TMR4->CC2 = -value;
// 			TMR3->CC2 = 0;
// 		}
// 		break;
// 	case MOTOR_3:
// 		if (value >= 0)
// 		{
// 			TMR3->CC3 = value;
// 			TMR4->CC3 = 0;
// 		}
// 		else
// 		{
// 			TMR4->CC3 = -value;
// 			TMR3->CC3 = 0;
// 		}
// 		break;
// 	case MOTOR_4:
// 		if (value >= 0)
// 		{
// 			TMR3->CC4 = value;
// 			TMR4->CC4 = 0;
// 		}
// 		else
// 		{
// 			TMR4->CC4 = -value;
// 			TMR3->CC4 = 0;
// 		}
// 		break;
// 	case MOTOR_5:
// 		if (value >= 0)
// 			TMR1->CC2 = value;
// 		break;
// 	case MOTOR_6:
// 		if (value >= 0)
// 			TMR1->CC3 = value;
// 		break;
// 	case MOTOR_7:
// 		if (value >= 0)
// 			TMR1->CC1 = value;
// 		break; //SERVO1
// 	case MOTOR_8:
// 		if (value >= 0)
// 			TMR2->CC1 = value;
// 		break; //SERVO2
// 	case MOTOR_9:
// 		if (value >= 0)
// 			TMR2->CC2 = value;
// 		break; //SERVO3
// 	default:
// 		break;
// 	}
// }
// void Set_Servo_Degree(SERVO_TYPE servo, uint8_t degree, uint8_t NewState)
// {
// 	if (degree > 180)
// 		degree = 180;
// 	if (degree <= 0)
// 		degree = 0;
// 	uint16_t value = degree * (TMR_CLOCK_PERIOD / 20) / 180 + 1000; //TMR_CLOCK_PERIOD
// 	switch (servo)
// 	{
// 	case SERVO_1:
// 		if (NewState)
// 		{
// 			if (value > 0)
// 				TMR1->CC1 = value;
// 		}
// 		else
// 		{
// 			TMR1->CC1 = 0;
// 		}
// 		break; //SERVO1
// 	case SERVO_2:
// 		if (NewState)
// 		{
// 			if (value > 0)
// 				TMR2->CC1 = value;
// 		}
// 		else
// 		{
// 			TMR2->CC1 = 0;
// 		}
// 		break; //SERVO2
// 	case SERVO_3:
// 		if (NewState)
// 		{
// 			if (value > 0)
// 				TMR2->CC2 = value;
// 		}
// 		else
// 		{
// 			TMR2->CC2 = 0;
// 		}
// 		break; //SERVO3
// 	default:
// 		break;
// 	}
// }
// void TMR1_OV_TMR10_IRQHandler(void)
// {
// 	if (TMR_GetINTStatus(TMR1, TMR_INT_Overflow) != RESET)
// 	{
// 		TMR_ClearITPendingBit(TMR1, TMR_INT_Overflow);
// 		if (TMR1->CC1 > 0)
// 		{
// 			// static bool flag;
// 			// if(flag)
// 			IO_Set(PC6);
// 			// else
// 			// IO_Reset(PC6);
// 			// flag = !flag;
// 		}
// 		if (TMR1->CC2 > 0)
// 		{
// 			IO_Set(PC7);
// 		}
// 		if (TMR1->CC3 > 0)
// 		{
// 			IO_Set(PC8);
// 		}
// 		if (TMR1->CC4 > 0)
// 		{
// 			IO_Set(PC9);
// 		}
// 	}
// }
// void TMR1_CC_IRQHandler(void)
// {
// 	if (TMR_GetINTStatus(TMR1, TMR_INT_CC1) != RESET)
// 	{
// 		TMR_ClearITPendingBit(TMR1, TMR_INT_CC1);
// 		IO_Reset(PC6); //M5->PA12，输出低电平
// 	}
// 	if (TMR_GetINTStatus(TMR1, TMR_INT_CC2) != RESET)
// 	{
// 		TMR_ClearITPendingBit(TMR1, TMR_INT_CC2);
// 		IO_Reset(PC7); //M5->PA12，输出低电平
// 	}
// 	if (TMR_GetINTStatus(TMR1, TMR_INT_CC3) != RESET)
// 	{
// 		TMR_ClearITPendingBit(TMR1, TMR_INT_CC3);
// 		IO_Reset(PC8); //M6->PA13,输出低电平
// 	}
// 	if (TMR_GetINTStatus(TMR1, TMR_INT_CC4) != RESET)
// 	{
// 		TMR_ClearITPendingBit(TMR1, TMR_INT_CC4);
// 		IO_Reset(PC9); //M6->PA13,输出低电平
// 	}
// }
